#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "stm32_robot/serial_driver.hpp"
#include "stm32_robot/protocol_parser.hpp"
#include <vector>
#include <cstdint>
#include <thread>
#include <mutex>
#include <memory>
#include <chrono>
#include <stdexcept>
#include <algorithm>
#include <atomic>
#include <Eigen/Dense>
#include <cmath>
#include <libserial/SerialPort.h>

using namespace std::chrono_literals;

class Stm32RobotNode : public rclcpp::Node {
public:
  Stm32RobotNode() 
      : Node("stm32_robot_node"), 
        serial_initialized_(false),
        needs_resend_(false),
        retry_count_(0),
        running_(true),
        frame_state_(HEADER),
        orientation_(Eigen::Quaterniond::Identity()),
        last_odom_update_(0, 0) {
    
    // 声明参数
    declare_parameter("device_name", "/dev/stm32");
    declare_parameter("baud_rate", 115200);
    declare_parameter("retry_interval_ms", 1000);
    declare_parameter("max_linear_speed", 0.5);
    declare_parameter("max_angular_speed", 1.0);
    declare_parameter("command_resend_interval", 20);
    declare_parameter("max_retry_count", 3);
    declare_parameter("odom_frame", "odom");
    declare_parameter("base_frame", "base_link");
    declare_parameter("imu_frame", "imu_link");
    declare_parameter("odom_update_rate", 50.0);
    declare_parameter("gyro_bias_x", 0.0);
    declare_parameter("gyro_bias_y", 0.0);
    declare_parameter("gyro_bias_z", 0.0);

    // 获取参数值
    odom_frame_ = get_parameter("odom_frame").as_string();
    base_frame_ = get_parameter("base_frame").as_string();
    imu_frame_ = get_parameter("imu_frame").as_string();
    double odom_update_rate = get_parameter("odom_update_rate").as_double();
    
    gyro_bias_ = Eigen::Vector3d(
      get_parameter("gyro_bias_x").as_double(),
      get_parameter("gyro_bias_y").as_double(),
      get_parameter("gyro_bias_z").as_double()
    );

    // 初始化串口
    InitializeSerialWithRetry();

    // 创建发布者
    imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
    odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
    static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*this);

     // 添加静态TF发布
    publish_static_tf(); // 确保IMU坐标系关系正确
    
    // 创建订阅者
    twist_sub_ = create_subscription<geometry_msgs::msg::Twist>(
        "/cmd_vel", 10,
        [this](geometry_msgs::msg::Twist::ConstSharedPtr msg) {
            HandleTwistCommand(msg);
        }
    );
    
    // 创建定时器
    auto update_period = std::chrono::duration<double>(1.0 / odom_update_rate);
    odom_timer_ = this->create_wall_timer(
        update_period,
        [this]() { odom_update_callback(); }
    );
    
    // 串口监控定时器
    serial_monitor_timer_ = create_wall_timer(
        500ms,
        [this]() { CheckSerialStatus(); }
    );
    
    // 命令重发定时器
    int resend_interval = get_parameter("command_resend_interval").as_int();
    resend_timer_ = create_wall_timer(
        std::chrono::milliseconds(resend_interval),
        [this]() { ResendLastCommand(); }
    );
    
    // 启动接收线程
    receiver_thread_ = std::thread(&Stm32RobotNode::receive_loop, this);
    
    RCLCPP_INFO(get_logger(), "Node initialized");
    RCLCPP_INFO(get_logger(), "Serial port: %s @ %d baud", 
               get_parameter("device_name").as_string().c_str(),
               get_parameter("baud_rate").as_int());
    RCLCPP_INFO(get_logger(), "Speed limits: Linear=%.2f m/s, Angular=%.2f rad/s", 
               get_parameter("max_linear_speed").as_double(),
               get_parameter("max_angular_speed").as_double());
    RCLCPP_INFO(get_logger(), "Gyro bias: X=%.4f, Y=%.4f, Z=%.4f", 
                gyro_bias_.x(), gyro_bias_.y(), gyro_bias_.z());
  }

  ~Stm32RobotNode() {
    running_ = false;
    if (receiver_thread_.joinable()) {
      receiver_thread_.join();
    }
    
    if (serial_driver_) {
      serial_driver_->Close();
    }
    
    RCLCPP_INFO(get_logger(), "Node shutdown");
  }

private:
  // 协议常量
  enum Command : uint8_t {
    CMD_IMU_DATA = 0xA0,
    CMD_SPEED_DATA = 0xA1,
    CMD_BATTERY = 0xA3
  };

  enum FrameState {
    HEADER,
    HEADER2,
    COMMAND,
    LENGTH,
    DATA,
    CHECKSUM,
    TAIL
  };

  struct VelocityCommand {
    int16_t vx = 0;
    int16_t vy = 0;
    int16_t vz = 0;
    rclcpp::Time timestamp;
  };

  void HandleTwistCommand(geometry_msgs::msg::Twist::ConstSharedPtr msg) {
    if (!serial_initialized_) {
      RCLCPP_WARN_THROTTLE(
          get_logger(), 
          *get_clock(), 
          1000, 
          "Serial not ready. Dropping velocity command"
      );
      
      if (retry_count_++ > get_parameter("max_retry_count").as_int()) {
        InitializeSerialWithRetry();
        retry_count_ = 0;
      }
      return;
    }
    
    try {
      const double max_lin = get_parameter("max_linear_speed").as_double();
      const double max_ang = get_parameter("max_angular_speed").as_double();
      
      const double clamped_lin_x = std::clamp(msg->linear.x, -max_lin, max_lin);
      const double clamped_lin_y = std::clamp(msg->linear.y, -max_lin, max_lin);
      const double clamped_ang_z = std::clamp(msg->angular.z, -max_ang, max_ang);
      
      last_command_.vx = SafeCastToInt16(clamped_lin_x * 1000);
      last_command_.vy = SafeCastToInt16(clamped_lin_y * 1000);
      last_command_.vz = SafeCastToInt16(clamped_ang_z * 1000);
      last_command_.timestamp = now();
      
      SendVelocityCommand();
      needs_resend_ = false;
      retry_count_ = 0;
    } catch (const std::exception& e) {
      RCLCPP_ERROR(get_logger(), "Command processing failed: %s", e.what());
      needs_resend_ = true;
    }
  }
  
  void SendVelocityCommand() {
    if (!serial_initialized_) return;
    
    try {
      auto data = Protocol::EncodeVelocity(
          last_command_.vx, 
          last_command_.vy, 
          last_command_.vz
      );
      serial_driver_->Write(data);
    } catch (const std::exception& e) {
      RCLCPP_ERROR(get_logger(), "Serial send failed: %s", e.what());
      throw;
    }
  }
  
  void ResendLastCommand() {
    if (!needs_resend_ || !serial_initialized_) return;
    
    auto now_time = now();
    if ((now_time - last_command_.timestamp).seconds() > 0.5) {
      needs_resend_ = false;
      return;
    }
    
    try {
      RCLCPP_WARN(get_logger(), "Resending last velocity command");
      SendVelocityCommand();
      needs_resend_ = false;
    } catch (...) {
      // 保留 needs_resend_ 为 true
    }
  }
  
  int16_t SafeCastToInt16(double value) {
    constexpr double max_val = std::numeric_limits<int16_t>::max();
    constexpr double min_val = std::numeric_limits<int16_t>::min();
    if (value > max_val || value < min_val) {
      throw std::range_error("Value out of int16 range");
    }
    return static_cast<int16_t>(value);
  }

  void InitializeSerialWithRetry() {
    const int retry_interval = get_parameter("retry_interval_ms").as_int();
    
    while (rclcpp::ok() && !serial_initialized_) {
      try {
        // 获取参数
        const std::string device_name = get_parameter("device_name").as_string();
        const int baud_rate = get_parameter("baud_rate").as_int();

        serial_driver_ = std::make_unique<SerialDriver>();
        serial_driver_->Setup(device_name, baud_rate);
        
        serial_initialized_ = true;
        RCLCPP_INFO(get_logger(), "Serial port initialized: %s @ %d baud", 
                  device_name.c_str(), baud_rate);
      } catch (const std::exception& e) {
        RCLCPP_ERROR(
            get_logger(),
            "Serial init failed: %s. Retrying in %d ms...",
            e.what(),
            retry_interval
        );
        std::this_thread::sleep_for(std::chrono::milliseconds(retry_interval));
      }
    }
  }

  void CheckSerialStatus() {
    if (serial_driver_ && !serial_driver_->IsOpen() && serial_initialized_) {
      RCLCPP_WARN(get_logger(), "Serial connection lost! Attempting reconnection...");
      serial_initialized_ = false;
      InitializeSerialWithRetry();
    }
  }

  void receive_loop() {
    while (running_ && rclcpp::ok()) {
      try {
        if (!serial_initialized_) {
          std::this_thread::sleep_for(100ms);
          continue;
        }
        
        // uint8_t byte;
        // size_t bytes_read = 0;
        // {
        //   // 使用超时读取避免永久阻塞
        //   bytes_read = serial_driver_->IsOpen() ? 1 : 0;
        //   if (bytes_read > 0) {
        //     // 实际读取将在process_byte中处理
        //     process_byte(byte);
        //   }
        // }
        uint8_t byte;
        bool read_success = serial_driver_->ReadByte(byte, 100); // 100ms超时
        if (read_success) {
          {
            std::lock_guard<std::mutex> lock(frame_mutex_);
            process_byte(byte);
          }
        }
      } catch (const std::exception& e) {
        RCLCPP_ERROR(get_logger(), "Receive error: %s", e.what());
      }
    }
  }

  void process_byte(uint8_t byte) {
    if (frame_buffer_.size() == 0 && byte == Protocol::FRAME_HEADER) {
      frame_state_ = HEADER;
    } else if (frame_state_ == HEADER && byte == Protocol::FRAME_HEADER) {
      frame_buffer_.clear();
      frame_buffer_.push_back(Protocol::FRAME_HEADER);
      frame_buffer_.push_back(Protocol::FRAME_HEADER);
      calc_checksum_ = 0;
      frame_state_ = COMMAND;
      return;
    }

    switch (frame_state_) {
      case HEADER:
        if (byte == Protocol::FRAME_HEADER) {
          frame_buffer_.push_back(byte);
          frame_state_ = HEADER2;
        } else {
          reset_frame_state();
        }
        break;

      case HEADER2:
        if (byte == Protocol::FRAME_HEADER) {
          frame_buffer_.push_back(byte);
          frame_state_ = COMMAND;
        } else {
          reset_frame_state();
        }
        break;

      case COMMAND:
        current_command_ = byte;
        frame_buffer_.push_back(byte);
        calc_checksum_ += byte;
        frame_state_ = LENGTH;
        break;

      case LENGTH:
        data_length_ = byte;
        frame_buffer_.push_back(byte);
        calc_checksum_ += byte;
        data_counter_ = 0;
        frame_state_ = (data_length_ > 0) ? DATA : CHECKSUM;
        break;

      case DATA:
        frame_buffer_.push_back(byte);
        calc_checksum_ += byte;
        if (++data_counter_ >= data_length_) {
          frame_state_ = CHECKSUM;
        }
        break;

      case CHECKSUM:
        if (byte == calc_checksum_) {
          frame_buffer_.push_back(byte);
          frame_state_ = TAIL;
        } else {
          RCLCPP_WARN(get_logger(), "Checksum error: expected 0x%02X got 0x%02X", calc_checksum_, byte);
          reset_frame_state();
        }
        break;

      case TAIL:
        if (byte == Protocol::FRAME_TAIL) {
          frame_buffer_.push_back(byte);
          parse_frame();
        } else {
          RCLCPP_WARN(get_logger(), "Frame tail error: expected 0x%02X got 0x%02X", Protocol::FRAME_TAIL, byte);
        }
        reset_frame_state();
        break;
    }
  }

  void reset_frame_state() {
    frame_buffer_.clear();
    frame_state_ = HEADER;
    calc_checksum_ = 0;
    data_length_ = 0;
    data_counter_ = 0;
  }

  void parse_frame() {
    if (frame_buffer_.size() < static_cast<size_t>(6)) {
      RCLCPP_WARN(get_logger(), "Frame too short");
      return;
    }

    const uint8_t* data = frame_buffer_.data() + 4;
    auto current_time = this->now();

    switch (current_command_) {
      case CMD_IMU_DATA:
        if (data_length_ == 28) {
          float ax = read_float_le(data + 4);
          float ay = read_float_le(data + 8);
          float az = read_float_le(data + 12);
          float gx = read_float_le(data + 16);
          float gy = read_float_le(data + 20);
          float gz = read_float_le(data + 24);

          {
            std::lock_guard<std::mutex> lock(imu_mutex_);
            last_accel_ = Eigen::Vector3d(ax, ay, az);
            last_gyro_ = Eigen::Vector3d(gx, gy, gz);
            last_imu_time_ = current_time;
          }

          process_imu_data(current_time);
        } else {
          RCLCPP_WARN(get_logger(), "IMU data length error: %d != 28", data_length_);
        }
        break;

      case CMD_SPEED_DATA:
        if (data_length_ == 6) {
          int16_t vx = read_i16_be(data);
          int16_t vy = read_i16_be(data + 2);
          int16_t vz = read_i16_be(data + 4);

          {
            std::lock_guard<std::mutex> lock(odom_mutex_);
            current_velocity_.linear.x = vx / 1000.0;
            current_velocity_.linear.y = vy / 1000.0;
            current_velocity_.angular.z = vz / 1000.0;
            last_vel_time_ = current_time;
          }
        } else {
          RCLCPP_WARN(get_logger(), "Speed data length error: %d != 6", data_length_);
        }
        break;

      case CMD_BATTERY:
        if (data_length_ == 2) {
          uint16_t voltage = read_u16_le(data);
          RCLCPP_INFO_THROTTLE(
            get_logger(), *get_clock(), 5000,
            "Battery voltage: %.2f V", voltage / 1000.0);
        } else {
          RCLCPP_WARN(get_logger(), "Battery data length error: %d != 2", data_length_);
        }
        break;

      default:
        RCLCPP_WARN(get_logger(), "Unknown command: 0x%02X", current_command_);
        break;
    }
  }

  void process_imu_data(rclcpp::Time timestamp) {
    Eigen::Vector3d accel, gyro;
    {
      std::lock_guard<std::mutex> lock(imu_mutex_);
      accel = last_accel_;
      gyro = last_gyro_;
    }

    static Eigen::Vector3d last_filtered_accel = accel;
    static Eigen::Vector3d last_filtered_gyro = gyro;
    static const double alpha = 0.2;
    
    Eigen::Vector3d filtered_accel = alpha * accel + (1 - alpha) * last_filtered_accel;
    Eigen::Vector3d filtered_gyro = alpha * gyro + (1 - alpha) * last_filtered_gyro;
    
    last_filtered_accel = filtered_accel;
    last_filtered_gyro = filtered_gyro;

    Eigen::Vector3d gyro_corrected = filtered_gyro - gyro_bias_;


    // ===== 添加方向更新（原代码未更新orientation_）=====
    static rclcpp::Time last_imu_time(0, 0);
    if (last_imu_time.seconds() > 0) {
      double dt = (timestamp - last_imu_time).seconds();
      
      // 简化的陀螺仪积分（实际应用中应使用更精确的算法）
      Eigen::Vector3d gyro_rad = gyro_corrected * M_PI/180.0; // 转换为弧度
      Eigen::Quaterniond delta_q = Eigen::Quaterniond(
          1, 
          gyro_rad.x() * dt / 2,
          gyro_rad.y() * dt / 2,
          gyro_rad.z() * dt / 2
      );
      
      orientation_ = (orientation_ * delta_q).normalized();
    }
    last_imu_time = timestamp;

    auto imu_msg = sensor_msgs::msg::Imu();
    imu_msg.header.stamp = timestamp;
    imu_msg.header.frame_id = imu_frame_;
    
    imu_msg.orientation_covariance = {0.05, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.05};
    imu_msg.angular_velocity_covariance = {0.002, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.002};
    imu_msg.linear_acceleration_covariance = {0.03, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.03};

    imu_msg.orientation.x = orientation_.x();
    imu_msg.orientation.y = orientation_.y();
    imu_msg.orientation.z = orientation_.z();
    imu_msg.orientation.w = orientation_.w();
    
    imu_msg.angular_velocity.x = gyro_corrected.x();
    imu_msg.angular_velocity.y = gyro_corrected.y();
    imu_msg.angular_velocity.z = gyro_corrected.z();
    
    imu_msg.linear_acceleration.x = filtered_accel.x();
    imu_msg.linear_acceleration.y = filtered_accel.y();
    imu_msg.linear_acceleration.z = filtered_accel.z();
    
    imu_pub_->publish(imu_msg);
  }

  void publish_static_tf() {
    geometry_msgs::msg::TransformStamped imu_tf;
    imu_tf.header.stamp = this->now();
    imu_tf.header.frame_id = base_frame_;
    imu_tf.child_frame_id = imu_frame_;
    
    imu_tf.transform.translation.x = 0.0;
    imu_tf.transform.translation.y = 0.0;
    imu_tf.transform.translation.z = 0.1;
    
    tf2::Quaternion q_imu;
    q_imu.setRPY(0, 0, 0);
    imu_tf.transform.rotation = tf2::toMsg(q_imu);
    
    static_tf_broadcaster_->sendTransform(imu_tf);
    RCLCPP_INFO(get_logger(), "Static TF published: %s -> %s", base_frame_.c_str(), imu_frame_.c_str());
  }

  void odom_update_callback() {
    geometry_msgs::msg::Twist current_vel;
    rclcpp::Time vel_time;
    
    {
      std::lock_guard<std::mutex> lock(odom_mutex_);
      current_vel = current_velocity_;
      vel_time = last_vel_time_;
    }

    rclcpp::Time current_time = this->now();
    
    if (last_odom_update_.nanoseconds() == 0) {
      last_odom_update_ = current_time;
      return;
    }
    
    rclcpp::Duration dt_duration = current_time - last_odom_update_;
    double dt = dt_duration.seconds();
    last_odom_update_ = current_time;
    
    double vx = current_vel.linear.x;
    double vy = current_vel.linear.y;
    double wz = current_vel.angular.z;
    
    double delta_x = vx * dt;
    double delta_y = vy * dt;
    
    x_ += delta_x;
    y_ += delta_y;
    theta_ += wz * dt;
    
    tf2::Quaternion q;
    q.setRPY(0, 0, theta_);
    
    auto odom_msg = nav_msgs::msg::Odometry();
    odom_msg.header.stamp = current_time;
    odom_msg.header.frame_id = odom_frame_;
    odom_msg.child_frame_id = base_frame_;
    
    std::array<double, 36> pose_covariance = {0};
    pose_covariance[0]  = 0.1;
    pose_covariance[7]  = 0.1;
    pose_covariance[14] = 0.01;
    pose_covariance[21] = 0.05;
    pose_covariance[28] = 0.05;
    pose_covariance[35] = 0.01;
    odom_msg.pose.covariance = pose_covariance;
    
    std::array<double, 36> twist_covariance = {0};
    twist_covariance[0]  = 0.02;
    twist_covariance[7]  = 0.02;
    twist_covariance[14] = 0.01;
    twist_covariance[21] = 0.01;
    twist_covariance[28] = 0.01;
    twist_covariance[35] = 0.03;
    odom_msg.twist.covariance = twist_covariance;

    odom_msg.pose.pose.position.x = x_;
    odom_msg.pose.pose.position.y = y_;
    odom_msg.pose.pose.position.z = 0.0;
    odom_msg.pose.pose.orientation = tf2::toMsg(q);
    
    odom_msg.twist.twist = current_vel;
    
    odom_pub_->publish(odom_msg);
    
    geometry_msgs::msg::TransformStamped odom_tf;
    odom_tf.header.stamp = current_time;
    odom_tf.header.frame_id = odom_frame_;
    odom_tf.child_frame_id = base_frame_;
    odom_tf.transform.translation.x = x_;
    odom_tf.transform.translation.y = y_;
    odom_tf.transform.translation.z = 0.0;
    odom_tf.transform.rotation = tf2::toMsg(q);
    
    tf_broadcaster_->sendTransform(odom_tf);
  }

  uint16_t read_u16_le(const uint8_t* data) {
    return static_cast<uint16_t>(data[0]) | (static_cast<uint16_t>(data[1]) << 8);
  }

  int16_t read_i16_be(const uint8_t* data) {
    return static_cast<int16_t>((data[0] << 8) | data[1]);
  }

  float read_float_le(const uint8_t* data) {
    uint32_t tmp = static_cast<uint32_t>(data[0]) |
                  (static_cast<uint32_t>(data[1]) << 8) |
                  (static_cast<uint32_t>(data[2]) << 16) |
                  (static_cast<uint32_t>(data[3]) << 24);
    return *reinterpret_cast<float*>(&tmp);
  }

  // 成员变量
  std::unique_ptr<SerialDriver> serial_driver_;
  bool serial_initialized_;
  bool needs_resend_;
  int retry_count_;
  VelocityCommand last_command_;
  
  // 接收处理相关
  std::atomic<bool> running_;
  FrameState frame_state_;
  std::thread receiver_thread_;
  
  std::mutex frame_mutex_;
  std::mutex imu_mutex_;
  std::mutex odom_mutex_;
  
  uint8_t current_command_;
  uint8_t data_length_;
  uint16_t data_counter_;
  uint8_t calc_checksum_;
  std::vector<uint8_t> frame_buffer_;

  // 传感器数据
  Eigen::Quaterniond orientation_;
  Eigen::Vector3d gyro_bias_;
  Eigen::Vector3d last_accel_;
  Eigen::Vector3d last_gyro_;
  rclcpp::Time last_imu_time_{0, 0};

  // 里程计
  double x_ = 0.0;
  double y_ = 0.0;
  double theta_ = 0.0;
  geometry_msgs::msg::Twist current_velocity_;
  rclcpp::Time last_vel_time_{0, 0};
  rclcpp::Time last_odom_update_{0, 0};
  std::string odom_frame_;
  std::string base_frame_;
  std::string imu_frame_;

  // ROS2 接口
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
  
  rclcpp::TimerBase::SharedPtr serial_monitor_timer_;
  rclcpp::TimerBase::SharedPtr resend_timer_;
  rclcpp::TimerBase::SharedPtr odom_timer_;
};

std::vector<uint8_t> Protocol::EncodeVelocity(int16_t vx, int16_t vy, int16_t vz) {
    std::vector<uint8_t> frame;
    
    // 双帧头
    frame.push_back(FRAME_HEADER);
    frame.push_back(FRAME_HEADER);
    
    // 命令码（速度控制）
    frame.push_back(CMD_VELOCITY);
    
    // 数据长度
    frame.push_back(DATA_LENGTH_VEL);
    
    // 数据体（大端序）
    auto push_int16 = [&](int16_t value) {
        frame.push_back(static_cast<uint8_t>((value >> 8) & 0xFF));
        frame.push_back(static_cast<uint8_t>(value & 0xFF));
    };
    push_int16(vx);
    push_int16(vy);
    push_int16(vz);
    
    // 校验和（从命令码开始累加）
    uint8_t checksum = 0;
    for (size_t i = 2; i < frame.size(); ++i) {
        checksum += frame[i];
    }
    frame.push_back(checksum);
    
    // 帧尾
    frame.push_back(FRAME_TAIL);
    
    return frame;
}



int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Stm32RobotNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}